


#include "RB_DCMOTORONBOARD.h"




RB_DcmotorOnBoard::RB_DcmotorOnBoard_Init(void)
{ 

     pinMode(MOTOR_PWM0, OUTPUT);  
     pinMode(MOTOR_PWM1, OUTPUT);
     pinMode(MOTOR_PWM2, OUTPUT);  
     pinMode(MOTOR_PWM3, OUTPUT);  

  
      TCCR0A = 0XA3;
      TCCR0B = 0X03;
  
      TCCR1A=0xAB;           //A、B、C通道非反转波形
      TCCR1B=0x02;           //8分频，计数器设置为10位

      OCR0A = 0;
      OCR0B = 0;
      
      OCR1A  = 0;
      OCR1B  = 0;
      
}

void RB_DcmotorOnBoard::RB_DcmotorOnBoardM1_Run(int32_t speed)
{    
  
      bool M_Dir = true;
      speed = constrain(speed,-99,99);
      if(speed < 0 ) {
            M_Dir = false;
      }
      else {
            M_Dir = true ;
      }
      
      if(M_Dir == true) {
           speed = abs(speed)*256/100;
           OCR0A = speed ; OCR0B = 0;
       }  
      else {
           speed = abs(speed)*256/100;
           OCR0A = 0; OCR0B = speed; 
       }
  
}

void RB_DcmotorOnBoard::RB_DcmotorOnBoardM2_Run(int32_t speed)
{  
    
      bool M_Dir = true;
      speed = constrain(speed,-99,99);
      if(speed < 0 ) {
            M_Dir = false;
      }
      else {
            M_Dir = true ;
      } 
      
      if(M_Dir == true) {
         speed =  abs(speed)*1024/100; 
          OCR1A = speed ; OCR1B = 0;
      }  
      else {
          speed =  abs(speed)*1024/100; 
          OCR1A = 0 ; OCR1B = speed; 
      }
}

void RB_DcmotorOnBoard::run(int32_t speed1,int32_t speed2)
 {
     RB_DcmotorOnBoard::RB_DcmotorOnBoardM1_Run(speed1);
     RB_DcmotorOnBoard::RB_DcmotorOnBoardM2_Run(speed2);
  }
void RB_DcmotorOnBoard::stop(void)
 {
     RB_DcmotorOnBoard::RB_DcmotorOnBoardM1_Run(0);
     RB_DcmotorOnBoard::RB_DcmotorOnBoardM2_Run(0);
  }
